#include "StdAfx.h"
#include "DdsUtil.h"
namespace dds
{
CDdsUtil::CDdsUtil(void)
{
}

CDdsUtil::~CDdsUtil(void)
{
}

void			CDdsUtil::SetFocusData( int nfocus, int nWatertemp, int nSalinity, int nFocusMode , int nSN)
{
	m_nFocus = nfocus;
	m_nWaterTemp = nWatertemp;
	m_nSalinity = nSalinity;
	m_nFocusMode = nFocusMode;
	m_nSN = nSN;

}
void		CDdsUtil::InitFocusCoefficients(void)
{

	// R = range (meters), T = temperature (degC), S = salinity (ppt), F = focus mode (0 = center weighted, 1 = averaged across beams)
	// S=0, F=0

	m_fFocusPos[0][0][0][0] = (const float)17.660;		// R=1,  T=5,  S=0,  F=0
	m_fFocusPos[1][0][0][0] = (const float)19.070;		// R=2,  T=5,  S=0,  F=0
	m_fFocusPos[2][0][0][0] = (const float)19.510;		// R=3,  T=5,  S=0,  F=0
	m_fFocusPos[3][0][0][0] = (const float)19.720;		// R=4,  T=5,  S=0,  F=0
	m_fFocusPos[4][0][0][0] = (const float)19.940;		// R=6,  T=5,  S=0,  F=0
	m_fFocusPos[5][0][0][0] = (const float)20.050;		// R=8,  T=5,  S=0,  F=0
	m_fFocusPos[6][0][0][0] = (const float)20.200;		// R=16, T=5,  S=0,  F=0
	m_fFocusPos[7][0][0][0] = (const float)20.270;		// R=32, T=5,  S=0,  F=0

	m_fFocusPos[0][1][0][0] = (const float)17.540;		// R=1,  T=15,  S=0,  F=0
	m_fFocusPos[1][1][0][0] = (const float)19.060;		// R=2,  T=15,  S=0,  F=0
	m_fFocusPos[2][1][0][0] = (const float)19.540;		// R=3,  T=15,  S=0,  F=0
	m_fFocusPos[3][1][0][0] = (const float)19.770;		// R=4,  T=15,  S=0,  F=0
	m_fFocusPos[4][1][0][0] = (const float)20.000;		// R=6,  T=15,  S=0,  F=0
	m_fFocusPos[5][1][0][0] = (const float)20.110;		// R=8,  T=15,  S=0,  F=0
	m_fFocusPos[6][1][0][0] = (const float)20.280;		// R=16, T=15,  S=0,  F=0
	m_fFocusPos[7][1][0][0] = (const float)20.360;		// R=32, T=15,  S=0,  F=0

	m_fFocusPos[0][2][0][0] = (const float)17.550;		// R=1,  T=25,  S=0,  F=0
	m_fFocusPos[1][2][0][0] = (const float)19.180;		// R=2,  T=25,  S=0,  F=0
	m_fFocusPos[2][2][0][0] = (const float)19.680;		// R=3,  T=25,  S=0,  F=0
	m_fFocusPos[3][2][0][0] = (const float)19.930;		// R=4,  T=25,  S=0,  F=0
	m_fFocusPos[4][2][0][0] = (const float)20.170;		// R=6,  T=25,  S=0,  F=0
	m_fFocusPos[5][2][0][0] = (const float)20.290;		// R=8,  T=25,  S=0,  F=0
	m_fFocusPos[6][2][0][0] = (const float)20.460;		// R=16, T=25,  S=0,  F=0
	m_fFocusPos[7][2][0][0] = (const float)20.540;		// R=32, T=25,  S=0,  F=0

	// S=15, F=0

	m_fFocusPos[0][0][1][0] = (const float)17.510;		// R=1,  T=5,  S=15,  F=0
	m_fFocusPos[1][0][1][0] = (const float)18.965;		// R=2,  T=5,  S=15,  F=0
	m_fFocusPos[2][0][1][0] = (const float)19.430;		// R=3,  T=5,  S=15,  F=0
	m_fFocusPos[3][0][1][0] = (const float)19.650;		// R=4,  T=5,  S=15,  F=0
	m_fFocusPos[4][0][1][0] = (const float)19.870;		// R=6,  T=5,  S=15,  F=0
	m_fFocusPos[5][0][1][0] = (const float)19.978;		// R=8,  T=5,  S=15,  F=0
	m_fFocusPos[6][0][1][0] = (const float)20.140;		// R=16, T=5,  S=15,  F=0
	m_fFocusPos[7][0][1][0] = (const float)20.220;		// R=32, T=5,  S=15,  F=0

	m_fFocusPos[0][1][1][0] = (const float)17.400;		// R=1,  T=15,  S=15,  F=0
	m_fFocusPos[1][1][1][0] = (const float)18.980;		// R=2,  T=15,  S=15,  F=0
	m_fFocusPos[2][1][1][0] = (const float)19.470;		// R=3,  T=15,  S=15,  F=0
	m_fFocusPos[3][1][1][0] = (const float)19.710;		// R=4,  T=15,  S=15,  F=0
	m_fFocusPos[4][1][1][0] = (const float)19.940;		// R=6,  T=15,  S=15,  F=0
	m_fFocusPos[5][1][1][0] = (const float)20.060;		// R=8,  T=15,  S=15,  F=0
	m_fFocusPos[6][1][1][0] = (const float)20.230;		// R=16, T=15,  S=15,  F=0
	m_fFocusPos[7][1][1][0] = (const float)20.310;		// R=32, T=15,  S=15,  F=0

	m_fFocusPos[0][2][1][0] = (const float)17.430;		// R=1,  T=25,  S=15,  F=0
	m_fFocusPos[1][2][1][0] = (const float)19.110;		// R=2,  T=25,  S=15,  F=0
	m_fFocusPos[2][2][1][0] = (const float)19.640;		// R=3,  T=25,  S=15,  F=0
	m_fFocusPos[3][2][1][0] = (const float)19.880;		// R=4,  T=25,  S=15,  F=0
	m_fFocusPos[4][2][1][0] = (const float)20.130;		// R=6,  T=25,  S=15,  F=0
	m_fFocusPos[5][2][1][0] = (const float)20.250;		// R=8,  T=25,  S=15,  F=0
	m_fFocusPos[6][2][1][0] = (const float)20.430;		// R=16, T=25,  S=15,  F=0
	m_fFocusPos[7][2][1][0] = (const float)20.520;		// R=32, T=25,  S=15,  F=0

	// S=35, F=0

	m_fFocusPos[0][0][2][0] = (const float)17.300;		// R=1,  T=5,  S=35,  F=0
	m_fFocusPos[1][0][2][0] = (const float)18.830;		// R=2,  T=5,  S=35,  F=0
	m_fFocusPos[2][0][2][0] = (const float)19.320;		// R=3,  T=5,  S=35,  F=0
	m_fFocusPos[3][0][2][0] = (const float)19.550;		// R=4,  T=5,  S=35,  F=0
	m_fFocusPos[4][0][2][0] = (const float)19.780;		// R=6,  T=5,  S=35,  F=0
	m_fFocusPos[5][0][2][0] = (const float)19.890;		// R=8,  T=5,  S=35,  F=0
	m_fFocusPos[6][0][2][0] = (const float)20.060;		// R=16, T=5,  S=35,  F=0
	m_fFocusPos[7][0][2][0] = (const float)20.150;		// R=32, T=5,  S=35,  F=0

	m_fFocusPos[0][1][2][0] = (const float)17.200;		// R=1,  T=15,  S=35,  F=0
	m_fFocusPos[1][1][2][0] = (const float)18.860;		// R=2,  T=15,  S=35,  F=0
	m_fFocusPos[2][1][2][0] = (const float)19.380;		// R=3,  T=15,  S=35,  F=0
	m_fFocusPos[3][1][2][0] = (const float)19.630;		// R=4,  T=15,  S=35,  F=0
	m_fFocusPos[4][1][2][0] = (const float)19.870;		// R=6,  T=15,  S=35,  F=0
	m_fFocusPos[5][1][2][0] = (const float)19.990;		// R=8,  T=15,  S=35,  F=0
	m_fFocusPos[6][1][2][0] = (const float)20.170;		// R=16, T=15,  S=35,  F=0
	m_fFocusPos[7][1][2][0] = (const float)20.260;		// R=32, T=15,  S=35,  F=0

	m_fFocusPos[0][2][2][0] = (const float)17.250;		// R=1,  T=25,  S=35,  F=0
	m_fFocusPos[1][2][2][0] = (const float)19.020;		// R=2,  T=25,  S=35,  F=0
	m_fFocusPos[2][2][2][0] = (const float)19.560;		// R=3,  T=25,  S=35,  F=0
	m_fFocusPos[3][2][2][0] = (const float)19.820;		// R=4,  T=25,  S=35,  F=0
	m_fFocusPos[4][2][2][0] = (const float)20.070;		// R=6,  T=25,  S=35,  F=0
	m_fFocusPos[5][2][2][0] = (const float)20.210;		// R=8,  T=25,  S=35,  F=0
	m_fFocusPos[6][2][2][0] = (const float)20.380;		// R=16, T=25,  S=35,  F=0
	m_fFocusPos[7][2][2][0] = (const float)20.480;		// R=32, T=25,  S=35,  F=0

	// S=0, F=1

	m_fFocusPos[0][0][0][1] = (const float)17.580;		// R=1,  T=5,  S=0,  F=1
	m_fFocusPos[1][0][0][1] = (const float)18.850;		// R=2,  T=5,  S=0,  F=1
	m_fFocusPos[2][0][0][1] = (const float)19.260;		// R=3,  T=5,  S=0,  F=1
	m_fFocusPos[3][0][0][1] = (const float)19.460;		// R=4,  T=5,  S=0,  F=1
	m_fFocusPos[4][0][0][1] = (const float)19.660;		// R=6,  T=5,  S=0,  F=1
	m_fFocusPos[5][0][0][1] = (const float)19.750;		// R=8,  T=5,  S=0,  F=1
	m_fFocusPos[6][0][0][1] = (const float)19.900;		// R=16, T=5,  S=0,  F=1
	m_fFocusPos[7][0][0][1] = (const float)19.970;		// R=32, T=5,  S=0,  F=1

	m_fFocusPos[0][1][0][1] = (const float)17.740;		// R=1,  T=15,  S=0,  F=1
	m_fFocusPos[1][1][0][1] = (const float)18.820;		// R=2,  T=15,  S=0,  F=1
	m_fFocusPos[2][1][0][1] = (const float)19.270;		// R=3,  T=15,  S=0,  F=1
	m_fFocusPos[3][1][0][1] = (const float)19.480;		// R=4,  T=15,  S=0,  F=1
	m_fFocusPos[4][1][0][1] = (const float)19.700;		// R=6,  T=15,  S=0,  F=1
	m_fFocusPos[5][1][0][1] = (const float)19.800;		// R=8,  T=15,  S=0,  F=1
	m_fFocusPos[6][1][0][1] = (const float)19.960;		// R=16, T=15,  S=0,  F=1
	m_fFocusPos[7][1][0][1] = (const float)20.030;		// R=32, T=15,  S=0,  F=1

	m_fFocusPos[0][2][0][1] = (const float)17.690;		// R=1,  T=25,  S=0,  F=1
	m_fFocusPos[1][2][0][1] = (const float)18.910;		// R=2,  T=25,  S=0,  F=1
	m_fFocusPos[2][2][0][1] = (const float)19.380;		// R=3,  T=25,  S=0,  F=1
	m_fFocusPos[3][2][0][1] = (const float)19.610;		// R=4,  T=25,  S=0,  F=1
	m_fFocusPos[4][2][0][1] = (const float)19.830;		// R=6,  T=25,  S=0,  F=1
	m_fFocusPos[5][2][0][1] = (const float)19.940;		// R=8,  T=25,  S=0,  F=1
	m_fFocusPos[6][2][0][1] = (const float)20.110;		// R=16, T=25,  S=0,  F=1
	m_fFocusPos[7][2][0][1] = (const float)20.190;		// R=32, T=25,  S=0,  F=1

	// S=15, F=1

	m_fFocusPos[0][0][1][1] = (const float)17.760;		// R=1,  T=5,  S=15,  F=1
	m_fFocusPos[1][0][1][1] = (const float)18.750;		// R=2,  T=5,  S=15,  F=1
	m_fFocusPos[2][0][1][1] = (const float)19.180;		// R=3,  T=5,  S=15,  F=1
	m_fFocusPos[3][0][1][1] = (const float)19.380;		// R=4,  T=5,  S=15,  F=1
	m_fFocusPos[4][0][1][1] = (const float)19.590;		// R=6,  T=5,  S=15,  F=1
	m_fFocusPos[5][0][1][1] = (const float)19.680;		// R=8,  T=5,  S=15,  F=1
	m_fFocusPos[6][0][1][1] = (const float)19.840;		// R=16, T=5,  S=15,  F=1
	m_fFocusPos[7][0][1][1] = (const float)19.910;		// R=32, T=5,  S=15,  F=1

	m_fFocusPos[0][1][1][1] = (const float)17.660;		// R=1,  T=15,  S=15,  F=1
	m_fFocusPos[1][1][1][1] = (const float)18.730;		// R=2,  T=15,  S=15,  F=1
	m_fFocusPos[2][1][1][1] = (const float)19.190;		// R=3,  T=15,  S=15,  F=1
	m_fFocusPos[3][1][1][1] = (const float)19.420;		// R=4,  T=15,  S=15,  F=1
	m_fFocusPos[4][1][1][1] = (const float)19.640;		// R=6,  T=15,  S=15,  F=1
	m_fFocusPos[5][1][1][1] = (const float)19.740;		// R=8,  T=15,  S=15,  F=1
	m_fFocusPos[6][1][1][1] = (const float)19.900;		// R=16, T=15,  S=15,  F=1
	m_fFocusPos[7][1][1][1] = (const float)19.980;		// R=32, T=15,  S=15,  F=1

	m_fFocusPos[0][2][1][1] = (const float)17.610;		// R=1,  T=25,  S=15,  F=1
	m_fFocusPos[1][2][1][1] = (const float)18.840;		// R=2,  T=25,  S=15,  F=1
	m_fFocusPos[2][2][1][1] = (const float)19.320;		// R=3,  T=25,  S=15,  F=1
	m_fFocusPos[3][2][1][1] = (const float)19.560;		// R=4,  T=25,  S=15,  F=1
	m_fFocusPos[4][2][1][1] = (const float)19.790;		// R=6,  T=25,  S=15,  F=1
	m_fFocusPos[5][2][1][1] = (const float)19.900;		// R=8,  T=25,  S=15,  F=1
	m_fFocusPos[6][2][1][1] = (const float)20.070;		// R=16, T=25,  S=15,  F=1
	m_fFocusPos[7][2][1][1] = (const float)20.160;		// R=32, T=25,  S=15,  F=1

	// S=35, F=1

	m_fFocusPos[0][0][2][1] = (const float)17.650;		// R=1,  T=5,  S=35,  F=1
	m_fFocusPos[1][0][2][1] = (const float)18.620;		// R=2,  T=5,  S=35,  F=1
	m_fFocusPos[2][0][2][1] = (const float)19.060;		// R=3,  T=5,  S=35,  F=1
	m_fFocusPos[3][0][2][1] = (const float)19.280;		// R=4,  T=5,  S=35,  F=1
	m_fFocusPos[4][0][2][1] = (const float)19.490;		// R=6,  T=5,  S=35,  F=1
	m_fFocusPos[5][0][2][1] = (const float)19.590;		// R=8,  T=5,  S=35,  F=1
	m_fFocusPos[6][0][2][1] = (const float)19.750;		// R=16, T=5,  S=35,  F=1
	m_fFocusPos[7][0][2][1] = (const float)19.830;		// R=32, T=5,  S=35,  F=1

	m_fFocusPos[0][1][2][1] = (const float)17.510;		// R=1,  T=15,  S=35,  F=1
	m_fFocusPos[1][1][2][1] = (const float)18.610;		// R=2,  T=15,  S=35,  F=1
	m_fFocusPos[2][1][2][1] = (const float)19.090;		// R=3,  T=15,  S=35,  F=1
	m_fFocusPos[3][1][2][1] = (const float)19.330;		// R=4,  T=15,  S=35,  F=1
	m_fFocusPos[4][1][2][1] = (const float)19.550;		// R=6,  T=15,  S=35,  F=1
	m_fFocusPos[5][1][2][1] = (const float)19.670;		// R=8,  T=15,  S=35,  F=1
	m_fFocusPos[6][1][2][1] = (const float)19.830;		// R=16, T=15,  S=35,  F=1
	m_fFocusPos[7][1][2][1] = (const float)19.920;		// R=32, T=15,  S=35,  F=1

	m_fFocusPos[0][2][2][1] = (const float)17.480;		// R=1,  T=25,  S=35,  F=1
	m_fFocusPos[1][2][2][1] = (const float)18.740;		// R=2,  T=25,  S=35,  F=1
	m_fFocusPos[2][2][2][1] = (const float)19.250;		// R=3,  T=25,  S=35,  F=1
	m_fFocusPos[3][2][2][1] = (const float)19.490;		// R=4,  T=25,  S=35,  F=1
	m_fFocusPos[4][2][2][1] = (const float)19.730;		// R=6,  T=25,  S=35,  F=1
	m_fFocusPos[5][2][2][1] = (const float)19.840;		// R=8,  T=25,  S=35,  F=1
	m_fFocusPos[6][2][2][1] = (const float)20.020;		// R=16, T=25,  S=35,  F=1
	m_fFocusPos[7][2][2][1] = (const float)20.110;		// R=32, T=25,  S=35,  F=1
}


//Now compute the actual focus range based on the value of N (correct for roundoff errors):

double CDdsUtil::SetFocusRange()
{
	double	focus, range, zeroPos, cmPerCount;

	//CDidsonDoc* pDoc = GetDocument();
	//ASSERT_VALID(pDoc);

	switch ( m_nSN)
	{
	case 1:		// SPAWAR-1		15cm, FC-40
	case 2:		// SPAWAR-2		15cm, FC-40	
	case 3:		// APL			15cm, FC-40
	case 4:		// Deep Drone	15cm, FC-40
		zeroPos = 17.3;
		cmPerCount = .012549020;
		break;
	case 8:		// USGS			14cm, FC-40
	case 10:	// WHOI-2		14cm, FC-40
		zeroPos = 16.3;
		cmPerCount = .016470588;
		break;
	case 5:		// SPAWAR-3		14cm, FC-40
	case 6:		// WHOI-1		14cm, FC-40
	case 7:		// PNNL			14cm, FC-40
	case 9:		// Magna-Patch	14cm, FC-40
	case 11:	// EOD-1		14cm, FC-40
	case 12:	// EOD-2		14cm, FC-40
	case 13:	// EOD-3		14cm, FC-40
	case 14:	// Crawler		14cm, FC-40
		zeroPos = 16.15;
		cmPerCount = .017058824;
		break;
	case 15:	// Bluefin-1	14cm, FC-40, slit moved forward
	default:
		zeroPos = 17.25;
		cmPerCount = .012549020;
		break;
	}
	focus = cmPerCount*(double)m_nFocus + zeroPos;
	if (focus > 20.5)
	{
		focus = 20.5;				// limit travel to avoid far mechanical stop
		m_nFocus = (UINT)((focus - zeroPos)/cmPerCount + 0.5);
	}

	if (m_nFocus >= 250)		// FOCUS_LIMIT
		range = 99.00;
	else if (focus <= zeroPos)
		range = 0.71;
	else if (focus <= m_fFocusPos[0][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED])
		range = 0.71 + .29*((focus - zeroPos)/(m_fFocusPos[0][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED] - zeroPos));
	else if (focus <= m_fFocusPos[1][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED])
		range = 1.00 + 1.0*((focus - m_fFocusPos[0][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED])/(m_fFocusPos[1][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED] - m_fFocusPos[0][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED]));
	else if (focus <= m_fFocusPos[2][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED])
		range = 2.00 + 1.0*((focus - m_fFocusPos[1][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED])/(m_fFocusPos[2][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED] - m_fFocusPos[1][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED]));
	else if (focus <= m_fFocusPos[3][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED])
		range = 3.00 + 1.0*((focus - m_fFocusPos[2][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED])/(m_fFocusPos[3][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED] - m_fFocusPos[2][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED]));
	else if (focus <= m_fFocusPos[4][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED])
		range = 4.00 + 2.0*((focus - m_fFocusPos[3][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED])/(m_fFocusPos[4][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED] - m_fFocusPos[3][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED]));
	else if (focus <= m_fFocusPos[5][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED])
		range = 6.00 + 2.0*((focus - m_fFocusPos[4][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED])/(m_fFocusPos[5][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED] - m_fFocusPos[4][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED]));
	else if (focus <= m_fFocusPos[6][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED])
		range = 8.00 + 8.0*((focus - m_fFocusPos[5][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED])/(m_fFocusPos[6][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED] - m_fFocusPos[5][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED]));
	else if (focus <= m_fFocusPos[7][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED])
		range = 16.00 + 16.0*((focus - m_fFocusPos[6][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED])/(m_fFocusPos[7][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED] - m_fFocusPos[6][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED]));
	else
		range = 99.00;

	//TRACE("SetFocusRange(): S/N = %d, Home = %.2f cm, Factor = %.5f\n", pDoc->m_Header.m_nSN, zeroPos, cmPerCount);
	//TRACE("SetFocusRange(): focus = %.2f cm, nfocus = %d, range = %.2f m\n", focus, m_nFocus, range);
	return(range);
}


int CDdsUtil::GetFocus(double range)
{
	int     nfocus;
	double	focus, zeroPos, cmPerCount;

	//CDidsonDoc* pDoc = GetDocument();
	//ASSERT_VALID(pDoc);

	range = range < 1.0 ? 1.0 : range;	// limit focus to 1.00m

	switch (m_nSN)
	{
	case 1:		// SPAWAR-1		15cm, FC-40
	case 2:		// SPAWAR-2		15cm, FC-40	
	case 3:		// APL			15cm, FC-40
	case 4:		// Deep Drone	15cm, FC-40
		zeroPos = 17.3;
		cmPerCount = .012549020;
		break;
	case 8:		// USGS			14cm, FC-40
	case 10:	// WHOI-2		14cm, FC-40
		zeroPos = 16.3;
		cmPerCount = .016470588;
		break;
	case 5:		// SPAWAR-3		14cm, FC-40
	case 6:		// WHOI-1		14cm, FC-40
	case 7:		// PNNL			14cm, FC-40
	case 9:		// Magna-Patch	14cm, FC-40
	case 11:	// EOD-1		14cm, FC-40
	case 12:	// EOD-2		14cm, FC-40
	case 13:	// EOD-3		14cm, FC-40
	case 14:	// Crawler		14cm, FC-40
		zeroPos = 16.15;
		cmPerCount = .017058824;
		break;
	case 15:	// Bluefin-1	14cm, FC-40, slit moved forward
	default:
		zeroPos = 17.25;
		cmPerCount = .012549020;
		break;
	}

	if (range <= 2.00)
		focus = m_fFocusPos[0][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED] + ((range-1.00)/1.00) * (m_fFocusPos[1][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED] - m_fFocusPos[0][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED]);
	else if (range <= 3.00)
		focus = m_fFocusPos[1][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED] + ((range-2.00)/1.00) * (m_fFocusPos[2][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED] - m_fFocusPos[1][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED]);
	else if (range <= 4.00)
		focus = m_fFocusPos[2][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED] + ((range-3.00)/1.00) * (m_fFocusPos[3][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED] - m_fFocusPos[2][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED]);
	else if (range <= 6.00)
		focus = m_fFocusPos[3][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED] + ((range-4.00)/2.00) * (m_fFocusPos[4][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED] - m_fFocusPos[3][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED]);
	else if (range <= 8.00)
		focus = m_fFocusPos[4][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED] + ((range-6.00)/2.00) * (m_fFocusPos[5][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED] - m_fFocusPos[4][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED]);
	else if (range <= 16.00)
		focus = m_fFocusPos[5][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED] + ((range-8.00)/8.00) * (m_fFocusPos[6][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED] - m_fFocusPos[5][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED]);
	else if (range <= 32.00)
		focus = m_fFocusPos[6][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED] + ((range-16.00)/16.00) * (m_fFocusPos[7][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED] - m_fFocusPos[6][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED]);
	else if (range > 32.00)
		focus = m_fFocusPos[7][ m_nWaterTemp][ m_nSalinity][ m_nFocusMode & BEAM_AVERAGED];

	nfocus = (int)((focus-zeroPos)/cmPerCount + 0.5);
	//TRACE("GetFocus: range = %.2f m, focus = %.2f cm, nfocus = %d\n", range, focus, nfocus);
	if (nfocus < 5) nfocus = 5;
	if (nfocus > 250) nfocus = 250;

	return(nfocus);
}
}
